% A is skbeta_triang
syms A0 A1 A2 A3 A4 A5 A6 A7 A8 real
syms A9 A10 A11 A12 A13 A14 A15 A16 A17 real
syms A18 A19 A20 A21 A22 A23 A24 A25 A26 real
syms A27 A28 A29 A30 A31 A32 A33 A34 A35 real

syms Z0 Z1 Z2 Z3 Z4 Z5 Z6 Z7 Z8 real
syms Z9 Z10 Z11 Z12 Z13 Z14 Z15 Z16 Z17 real
syms Z18 Z19 Z20 Z21 Z22 Z23 Z24 Z25 Z26 real
syms Z27 Z28 Z29 Z30 Z31 Z32 Z33 Z34 Z35 real

syms r0 r1 r2 r3 real
syms g0 g1 g2 real
syms w0 w1 w2 real
syms m mass real
syms inertia real
syms s0 s1 real
syms beta0 beta1 beta2 real
syms p0 p1 p2 p3 p4 p5 p6 p7 p8 real
syms t0 t1 t2 t3 t4 t5 t6 t7 t8 real
syms z0 z1 z2 z3 z4 z5 z6 z7 z8 real
syms f0 f1 f2 real;
syms b0 b1 b2 real;
syms phi0 phi1 phi2 phi3 phi4 phi5 phi6 phi7 phi8 real
syms psi0 psi1 psi2 psi3 psi4 psi5 psi6 psi7 psi8 real
syms m0 m1 m2 real;
syms I0 I1 I2 I3 I4 I5 I6 I7 I8 real
syms I9 I10 I11 I12 I13 I14 I15 I16 I17 real
syms I18 I19 I20 I21 I22 I23 I24 I25 I26 real
syms I27 I28 I29 I30 I31 I32 I33 I34 I35 real
syms J0 J1 J2 J3 J4 J5 J6 J7 J8 real
syms im real;

J = [J0 J1 J2;
     J3 J4 J5;
     J6 J7 J8];
N_C_K = [J zeros(3);
        zeros(3) J];
I = [   I0  I1  I2  I3  I4  I5;
        I6  I7  I8  I9  I10 I11;
        I12 I13 I14 I15 I16 I17;
        I18 I19 I20 I21 I22 I23;
        I24 I25 I26 I27 I28 I29;
        I30 I31 I32 I33 I34 I35];

I6x5 = [ I0 I1 I2 I3 I4;
         I5 I6 I7 I8 I9;
         I10 I11 I12 I13 I14;
         I15 I16 I17 I18 I19;
         I20 I21 I22 I23 I24;
         I25 I26 I27 I28 I29];
     
 I5x5 = [ I0 I1 I2 I3 I4;
         I5 I6 I7 I8 I9;
         I10 I11 I12 I13 I14;
         I15 I16 I17 I18 I19;
         I20 I21 I22 I23 I24];
     
A6x5 = [ A0 A1 A2 A3 A4;
         A5 A6 A7 A8 A9;
         A10 A11 A12 A13 A14;
         A15 A16 A17 A18 A19;
         A20 A21 A22 A23 A24;
         A25 A26 A27 A28 A29];
         
    
    


 betaX = [0      -beta2  beta1;
         beta2  0       -beta0;
        -beta1  beta0   0];
rX  =   [0 -r2 r1;
         r2 0 -r0;
         -r1 r0 0];
wx = [0 -w2 w1;
      w2 0 -w0;
      -w1 w0 0];
     
skbeta = [eye(3,3) betaX;
          zeros(3,3) eye(3,3)];
Pkk = [1 0 0 0 0 0;
       0 1 0 0 0 0;
       0 0 1 0 0 0]';
H = [0 0 1 0 0 0]';
HN = N_C_K*H;
D = [1 0 0 0 0;
     0 1 0 0 0;
     0 0 0 0 0;
     0 0 1 0 0;
     0 0 0 1 0;
     0 0 0 0 1];
 DN = N_C_K*D;

M = [m0 m1 m2];
w = [w0 ;w1 ;w2];

g = [g0; g1 ; g2];
r2D = [r0;r1];
g2D = [g0;g1];

A = [   A0  A1  A2  A3  A4  A5;
        A6  A7  A8  A9  A10 A11;
        A12 A13 A14 A15 A16 A17;
        A18 A19 A20 A21 A22 A23;
        A24 A25 A26 A27 A28 A29;
        A30 A31 A32 A33 A34 A35];  
    
   
     
 A6x1=[ A0; A1;A2;A3;A4;A5];
 Z6x1 = [Z0;Z1;Z2;Z3;Z4;Z5];
 Z3x1 = [Z0;Z1;Z2];
        
A6x3=[   A0  A1  A2;
         A3  A4  A5;
         A6  A7  A8;
         A9  A10 A11;
         A12 A13 A14;
         A15 A16 A17];
     
 Z6x3=[   Z0  Z1  Z2;
         Z3  Z4  Z5;
         Z6  Z7  Z8;
         Z9  Z10 Z11;
         Z12 Z13 Z14;
         Z15 Z16 Z17];
    
In = [I0  I1  I2;
      I3  I4  I5;
      I6  I7  I8];



Sf2 = [eye(3,3) rX;
       zeros(3,3) eye(3,3)];
   
Sf1 = [eye(3,3) -rX;
       zeros(3,3) eye(3,3)];

iM = [In zeros(3);
    zeros(3) im*eye(3)];
phi11 = Sf1'*iM*Sf1;
phi12 = Sf1'*iM*Sf2;
phi21 = Sf2'*iM*Sf1;
phi22 = Sf2'*iM*Sf2;

% Wa = In*D'*A6x3;
% Wc = In*D'*(A6x1-Z6x1);
% P = I5x5*D'*(A6x1-Z6x1+[w;0;0;0]);
% P = phi22;
P = DN*I5x5*DN;
keyboard


%  
%  ccode(P,'file','test');
%  keyboard
%  [T1 T2 T3] = textread('test','%s%c%s');
%  
%  CHARS{1} = 'I';
%  CHARS{2} = 'J';
%  CHARS{3} = 't';
%  % CHARS{4} = 't';
%  % CHARS{6} ='Z';
%  [rowt,colt] = size(T1);
%  [rowc,colc] = size(CHARS);
%  
%  % Length of CHARS
%  for j = 1:colc
%          for i = 250:-1:0
%          % edit t
%          ch = char(CHARS{j});
%          T1 =strrep(T1,strcat(ch,num2str(i)),strcat(ch,'[',num2str(i),']'));
%          T3 =strrep(T3,strcat(ch,num2str(i)),strcat(ch,'[',num2str(i),']'));
%      end
%  end
%  T1 = strrep(T1,'MatrixWithNoName','m');
%  
%  count = 0;
%  for i=0:4
%      for j=0:4
%          T1 = strrep(T1,strcat('[',num2str(i),'][',num2str(j),']'),strcat('[',num2str(count),']'));
%          count = count+1;
%      end
%  end
%  
%  fid = fopen('op','w+');
%  
%  % Length of T1
%  for i=1:rowt
%      fprintf(fid,'%s=%s\n',char(T1(i)),char(T3(i)));
%  end
%  
%  fclose(fid);


